#include <iostream>
#include <math.h>
#include <random>
#include <eigen3/Eigen/Dense>
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Odometry.h>
#include "traj_utils/PositionCommand.h"
#include <control_msgs/JointControllerState.h>

ros::Subscriber cmd_sub, cmd_cam_yaw_sub;
ros::Publisher odom_pub, cam_pose_pub, cam_yaw_pub;
ros::Timer odom_timer, cam_pose_timer;

// DEBUG
ros::Publisher debug_yaw_pub;

/* Global Variables */
nav_msgs::Odometry odom;
Eigen::Matrix3d body2base, base2cam;
Eigen::Quaterniond q_world2body, q_body2cam, q_world2cam;
double cam_yaw_target = 0.0;
double cam_yaw = 0.0;

traj_utils::PositionCommand _cmd;
double _init_x, _init_y, _init_z;
bool rcv_cmd = false;
bool flag_update_cam_yaw = false;
bool flag_have_odom = false;

/* configuration */
constexpr double max_cam_yaw_dot = M_PI; // TODO MZY :改成可读的参数 rad/s
constexpr double t_odom_pub = 0.01;
constexpr double t_cam_pose_pub = 0.02;
constexpr double max_cam_yaw_delta = max_cam_yaw_dot * t_cam_pose_pub;
constexpr double max_cam_yaw = M_PI_2;
constexpr double min_cam_yaw = -max_cam_yaw;

void RcvPosCmdCb(const traj_utils::PositionCommand cmd)
{
  rcv_cmd = true;
  _cmd = cmd;
}

void CamYawCb(const geometry_msgs::PoseStampedConstPtr &msg)
{
  cam_yaw_target = msg->pose.position.x;
  flag_update_cam_yaw = true;
  /* limit the camera yaw */
  if (cam_yaw_target > max_cam_yaw)
  {
    cam_yaw_target = max_cam_yaw;
  }
  else if (cam_yaw_target < min_cam_yaw)
  {
    cam_yaw_target = min_cam_yaw;
  }
}

void pubOdom(const ros::TimerEvent &event)
{
  odom.header.stamp = ros::Time::now();
  odom.header.frame_id = "world";

  if (rcv_cmd)
  {
    odom.pose.pose.position.x = _cmd.position.x;
    odom.pose.pose.position.y = _cmd.position.y;
    odom.pose.pose.position.z = _cmd.position.z;

    Eigen::Vector3d alpha = Eigen::Vector3d(_cmd.acceleration.x,
                                            _cmd.acceleration.y,
                                            _cmd.acceleration.z) +
                            9.8 * Eigen::Vector3d(0, 0, 1);
    Eigen::Vector3d xC(cos(_cmd.yaw), sin(_cmd.yaw), 0);
    Eigen::Vector3d yC(-sin(_cmd.yaw), cos(_cmd.yaw), 0);
    Eigen::Vector3d xB = (yC.cross(alpha)).normalized();
    Eigen::Vector3d yB = (alpha.cross(xB)).normalized();
    Eigen::Vector3d zB = xB.cross(yB);
    Eigen::Matrix3d R;
    R.col(0) = xB;
    R.col(1) = yB;
    R.col(2) = zB;
    Eigen::Quaterniond q(R);
    odom.pose.pose.orientation.w = q.w();
    odom.pose.pose.orientation.x = q.x();
    odom.pose.pose.orientation.y = q.y();
    odom.pose.pose.orientation.z = q.z();

    odom.twist.twist.linear.x = _cmd.velocity.x;
    odom.twist.twist.linear.y = _cmd.velocity.y;
    odom.twist.twist.linear.z = _cmd.velocity.z;

    odom.twist.twist.angular.x = _cmd.acceleration.x;
    odom.twist.twist.angular.y = _cmd.acceleration.y;
    odom.twist.twist.angular.z = _cmd.acceleration.z;
  }
  else
  {
    odom.pose.pose.position.x = _init_x;
    odom.pose.pose.position.y = _init_y;
    odom.pose.pose.position.z = _init_z;

    odom.pose.pose.orientation.w = 1;
    odom.pose.pose.orientation.x = 0;
    odom.pose.pose.orientation.y = 0;
    odom.pose.pose.orientation.z = 0;

    odom.twist.twist.linear.x = 0.0;
    odom.twist.twist.linear.y = 0.0;
    odom.twist.twist.linear.z = 0.0;

    odom.twist.twist.angular.x = 0.0;
    odom.twist.twist.angular.y = 0.0;
    odom.twist.twist.angular.z = 0.0;
  }
  flag_have_odom = true;
  odom_pub.publish(odom);
}

/**
 * Coordinate：
 * 	world: X-forward Y-left Z-up
 * 	body: fix on UAV, X-forward Y-left Z-up
 *  base: camera base, have the same initial orientation as body, rotate about the z-axis of body
 *  cam: fix on base, X-right Y-down Z-forward
 * Matrix A2B: the coord B in A or move coord A to B
 */
void pubCameraPose(const ros::TimerEvent &event)
{
  if (!flag_have_odom)
    return;
  if (flag_update_cam_yaw)
  {
    if (abs(cam_yaw_target - cam_yaw) > max_cam_yaw_delta)
    {
      cam_yaw = cam_yaw_target > cam_yaw
                    ? (cam_yaw + max_cam_yaw_delta)
                    : (cam_yaw - max_cam_yaw_delta);
    }
    else
    {
      cam_yaw = cam_yaw_target;
      flag_update_cam_yaw = false;
    }
    body2base = Eigen::AngleAxisd(cam_yaw, Eigen::Vector3d(0.0, 0.0, 1.0));
    q_body2cam = body2base * base2cam;
  }
  q_world2body = Eigen::Quaterniond(odom.pose.pose.orientation.w,
                                    odom.pose.pose.orientation.x,
                                    odom.pose.pose.orientation.y,
                                    odom.pose.pose.orientation.z);
  q_world2cam = q_world2body * q_body2cam;

  geometry_msgs::PoseStamped camera_pose;
  camera_pose.header = odom.header;
  camera_pose.header.frame_id = "/map";
  camera_pose.pose.position.x = odom.pose.pose.position.x;
  camera_pose.pose.position.y = odom.pose.pose.position.y;
  camera_pose.pose.position.z = odom.pose.pose.position.z;
  camera_pose.pose.orientation.w = q_world2cam.w();
  camera_pose.pose.orientation.x = q_world2cam.x();
  camera_pose.pose.orientation.y = q_world2cam.y();
  camera_pose.pose.orientation.z = q_world2cam.z();
  cam_pose_pub.publish(camera_pose);

  control_msgs::JointControllerState camera_yaw;
  camera_yaw.header = odom.header;
  camera_yaw.process_value = cam_yaw;
  cam_yaw_pub.publish(camera_yaw);

  // DEBUG
  if (debug_yaw_pub.getNumSubscribers() > 0)
  {
    Eigen::Vector3d rot_x = q_world2body.toRotationMatrix().block(0, 0, 3, 1);
    geometry_msgs::PoseStamped debug_yaw;
    debug_yaw.header = odom.header;
    debug_yaw.pose.position.x = cam_yaw_target;
    debug_yaw.pose.position.y = cam_yaw;
    debug_yaw.pose.position.z = atan2(rot_x(1), rot_x(0));
    debug_yaw_pub.publish(debug_yaw);
  }
}

void init_camera()
{
  base2cam << 0.0, 0.0, 1.0,
      -1.0, 0.0, 0.0,
      0.0, -1.0, 0.0;
  q_body2cam = base2cam;
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "odom_generator");
  ros::NodeHandle nh("~");

  init_camera();

  nh.param("init_x", _init_x, 0.0);
  nh.param("init_y", _init_y, 0.0);
  nh.param("init_z", _init_z, 0.0);

  cmd_sub = nh.subscribe("command", 1, RcvPosCmdCb);
  cmd_cam_yaw_sub = nh.subscribe("yaw_cmd", 1, CamYawCb);
  odom_pub = nh.advertise<nav_msgs::Odometry>("odometry", 1);
  cam_yaw_pub = nh.advertise<control_msgs::JointControllerState>("camera_yaw", 1);
  cam_pose_pub = nh.advertise<geometry_msgs::PoseStamped>("camera_pose", 1);

  // DEBUG
  debug_yaw_pub = nh.advertise<geometry_msgs::PoseStamped>("debug/yaw", 1);

  odom_timer = nh.createTimer(ros::Duration(t_odom_pub), pubOdom);
  cam_pose_timer = nh.createTimer(ros::Duration(t_cam_pose_pub), pubCameraPose);

  ros::spin();

  return 0;
}